/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-02-22     Wangjiangbo       the first version
 */
#include <App/app_force_move.h>
#include <App/app_motorcontrol.h>
#include <App/app_io.h>
#include <Force/force.h>
#include <Chassis/chassis.h>

#define DBG_TAG "move"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

Chassis chassis;

enum RUN_STATE {
    TO_MC_IDLE = 0,
    MC_IDLE,
    MC_MOVE,
    MC_SENSOR_CALIBRATION,
};

enum RUN_CMD {
    CMD_NONE = 0,
    CMD_ENABLE = 1,
    CMD_DISABLE,
    CMD_CALIBRATION,
};

static rt_timer_t timer;
static rt_sem_t sem;
static int8_t cmd = 0;
static int32_t axit_exit[6] = {0};

 int twiceTauLPF=60;  // twiceTauLPF/2个周期滤波
static double lastfilter_force,last_force,current_force=0;
static double lastfilter_torque,last_torque,current_torque=0;

int force_status=0;
static void timeout (void *parameter)
{
    rt_sem_release(sem);
}

// 线程入口
static void thread_entry(void* parameter)
{
    int8_t move_cmd = 0;
    int8_t state = TO_MC_IDLE;
    int32_t axit_raw[6] = {0};
    int32_t axit_offset[6] = {0};
    int32_t axit_calib_cnt = 0;
    int32_t axit_sum[6] = {0};
    int32_t axit[6] = {0};
    iomap map = {0};

    int8_t force_cacli_status =0;

    uint16_t force_start_count=0;
    rt_thread_mdelay(3000);

    // 创建信号量
    sem = rt_sem_create("force sem", 0, RT_IPC_FLAG_FIFO);
    // 创建定时器  周期定时器
    timer = rt_timer_create("force time", timeout,
                             RT_NULL,
                             rt_tick_from_millisecond(30),
                             RT_TIMER_FLAG_PERIODIC);

    if (timer != RT_NULL)
        rt_timer_start(timer);

    LOG_D("助力线程开启");
    while (1)
    {

        if(rt_sem_take(sem, RT_WAITING_FOREVER) != RT_EOK)
            continue;

        {
            static int timeout = 0;
            if(++timeout > 10)
            {
                timeout = 0;
                //LOG_D("raw fx = %08d fy = %08d fz = %08d tx = %08d",  axit_raw[0],  axit_raw[1],  axit_raw[2],  axit_raw[3] );
                //LOG_D("pro fx = %08d fy = %08d fz = %08d tx = %08d",  axit[0],  axit[1],  axit[2],  axit[3] );
            }
        }

        rt_enter_critical();

        move_cmd = cmd;
//        cmd = CMD_NONE;

        rt_exit_critical();


        app_io_get_gpio_map(&map);
        force_get(axit_raw);

        for(int i = 0; i < 6 ; i++)
            axit[i] = axit_raw[i] - axit_offset[i];

        rt_enter_critical();
        memcpy(axit_exit, axit, sizeof(axit));
        rt_exit_critical();

        switch(state)
        {
        case TO_MC_IDLE:
            app_mc_disable();
            state = MC_IDLE;
            LOG_D("助力关闭");
            rt_thread_mdelay(1000);
            break;

        case MC_IDLE:   //空闲状态下进行传感器标定，只需要标定一次？？
            if(!force_cacli_status)
            {
                if(axit_calib_cnt == 0)
                    LOG_D("助力校准开启");

                for(int i = 0; i < 6 ; i++)
                    axit_sum[i] += axit_raw[i];

                if(++axit_calib_cnt > 10)//10
                {
                    for(int i = 0; i < 6 ; i++)
                        axit_offset[i] = axit_sum[i] / axit_calib_cnt;

                    memset(axit_sum, 0 , sizeof(axit_sum));
                    axit_calib_cnt = 0;

                    force_cacli_status = 1;

                    LOG_D("助力校准完成");
                    LOG_D("fx_offset = %08d, tx_offset = %08d", axit_offset[0],  axit_offset[3]);
                }
            }
            else
            {
                if((move_cmd == CMD_ENABLE)&&(map.key_st[e_key_drag] == 0))
                {
                    last_force =0;
                    last_torque =0;
                    lastfilter_force = 0;
                    lastfilter_torque = 0;

                    force_status=1;
                    force_start_count=0;
                    state = MC_MOVE;
                    app_mc_enable();
                    rt_thread_mdelay(200);
                }
            }
            break;
        case MC_MOVE:
            if(move_cmd == CMD_DISABLE)
                state = TO_MC_IDLE;

            if(map.key_st[e_key_drag] == 0)
            {
                struct WheelVelocity wheel_velocity;
                struct ChassisForceSensorValue chassis_force_sensor_value;

                if( force_start_count < 100)
                {
                    force_start_count++;
                    twiceTauLPF=200;
                }
                else{
                    twiceTauLPF=50;
                }
//                chassis_force_sensor_value.force = axit[0] / 1000.0f * -1.0;
//                chassis_force_sensor_value.torque = axit[3] / 1000.0f;
//
//                chassis.ChassisMoventForceGuide(chassis_force_sensor_value, wheel_velocity);
//                app_mc_run(LEFT_ID, wheel_velocity.left);
//                app_mc_run(RIGHT_ID, wheel_velocity.right);
//                {
//                    static int t = 0;
//
//                    if(++t > 10)
//                    {
//                        t = 0;
//                        LOG_D("fx = %d, tx = %d, left = %d right = %d", (int)chassis_force_sensor_value.force,(int)chassis_force_sensor_value.torque, (int)wheel_velocity.left,  (int)wheel_velocity.right);
//                        LOG_D("fx = %08d, tx = %08d, left = %08d right = %08d", axit[0],  axit[3], (int)wheel_velocity.left,  (int)wheel_velocity.right);
//                    }
                    current_force = axit[0] / 1000.0f * -1.0;
                    chassis_force_sensor_value.force=(current_force+last_force)/(twiceTauLPF+1)-lastfilter_force*(1-twiceTauLPF)/(1+twiceTauLPF);
                    last_force = current_force;
                    lastfilter_force = chassis_force_sensor_value.force;

                    current_torque = axit[3] / 1000.0f;
                    chassis_force_sensor_value.torque=(current_torque+last_torque)/(twiceTauLPF+1)-lastfilter_torque*(1-twiceTauLPF)/(1+twiceTauLPF);
                    last_torque = current_torque;
                    lastfilter_torque = chassis_force_sensor_value.torque;

                    chassis.ChassisMoventForceGuide(chassis_force_sensor_value, wheel_velocity);
                    app_mc_run(LEFT_ID, wheel_velocity.left);
                    app_mc_run(RIGHT_ID, wheel_velocity.right);
//                }
            }
            else
            {
                app_mc_run(LEFT_ID, 0);
                app_mc_run(RIGHT_ID, 0);
                app_mc_disable();

                force_status=0;
                force_start_count =0;
                state = TO_MC_IDLE;
            }
            break;
        default:
            break;
        }

    }
}

void app_force_get_axit(uint32_t* axit)
{
    rt_enter_critical();
    memcpy(axit, axit_exit, sizeof(axit_exit));
    rt_exit_critical();
}

void app_force_move_init(void)
{
#define THREAD_STACK_SIZE   1024
#define THREAD_PRIORITY     9
#define THREAD_TIMESLICE    9

    rt_thread_t tid = RT_NULL;



    // 创建线程
    tid = rt_thread_create("force control",
                            thread_entry, RT_NULL,
                            THREAD_STACK_SIZE,
                            THREAD_PRIORITY, THREAD_TIMESLICE);


    if (tid != RT_NULL)
        rt_thread_startup(tid);

}

void app_force_move_enable(void)
{
    rt_enter_critical();
    cmd = CMD_ENABLE;
    rt_exit_critical();
}
MSH_CMD_EXPORT(app_force_move_enable, enable);

void app_force_move_disable(void)
{
    rt_enter_critical();
    cmd = CMD_DISABLE;
    rt_exit_critical();
}
MSH_CMD_EXPORT(app_force_move_disable, disable);

void app_force_move_calibration(void)
{
    rt_enter_critical();
    cmd = CMD_CALIBRATION;
    rt_exit_critical();
}
/* 导出到 msh 命令列表中 */
MSH_CMD_EXPORT(app_force_move_calibration, 校准传感器);
